#include "motionItf.h"
#include "motion_debug.h"
#include <math.h>
#include "motion_struct.h"
#include "rtapi/rtapi.h"        /* RTAPI realtime OS API */
#include "rtapi/rtapi_string.h" /* memset */
#include "motion.h"
#include "errcode.h"
#include "mot_priv.h"
#include "rtdev/tio_intf_c.h"

ZucPose zucmotGetCur_Tooloffset() { return zucmotConfig->tool_offset; }
int zucmotGet_toppqueuelen() { return zucmotConfig->max_planning_queue_size; }
int zucmotGet_maxfwlen() { return zucmotConfig->max_fw_len; }
PmRpy zucmotGetCur_BaseOffset() { return zucmotConfig->base_offset; }
ZucPose zucmotGetCur_UserOffset() { return zucmotConfig->user_offset; }

double zucmotGet_maxFeedScale() { return zucmotConfig->maxFeedScale; }

int zucmotGet_TotalNumAIO(char busType)
{
    if (busType == IODevTYPE_PHYSIC)
    {
        return zucmotConfig->cabTotalNumAO;
    }

    if (busType == IODevTYPE_EXTIO)
    {
        return MAX_EXTIO_AI_NUM;
    }

    if (busType == IODevTYPE_SYNCH_MB_SLAVE)
    {
        return zucmotConfig->MB_Slave_TotalNumAIO;
    }
    if (busType == IODevTYPE_SYNCH_PN_DEV)
    {
        return zucmotConfig->PN_Dev_TotalNumAIO;
    }
    if (busType == IODevTYPE_SYNCH_EIP_ADPT)
    {
        if (zucmotStatus->eip_mode == 1)
        {
            return zucmotStatus->eip_ao_int_num + zucmotStatus->eip_ao_float_num;
        }
        else
        {
            return zucmotConfig->EIP_Adpt_TotalNumAIO;
        }
    }
    return 0;
}
int zucmotGet_TotalNumDIO(char busType)
{
    if (busType == IODevTYPE_PHYSIC)
    {  //
        return zucmotConfig->cabTotalNumDIO;
    }

    if (busType == IODevTYPE_TIO)
    {
        return zucmotConfig->TIO_TotalNumDIO;
    }

    if (busType & IODevTYPE_EXTIO)
    {
        return MAX_EXTIO_DO_NUM;
    }

    if (busType == IODevTYPE_SYNCH_MB_SLAVE)
    {
        return zucmotConfig->MB_Slave_TotalNumDIO;
    }
    if (busType == IODevTYPE_SYNCH_PN_DEV)
    {
        return zucmotConfig->PN_Dev_TotalNumDIO;
    }
    if (busType == IODevTYPE_SYNCH_EIP_ADPT)
    {
        if (zucmotStatus->eip_mode == 1)
        {
            return zucmotStatus->eip_do_num;
        }
        else
        {
            return zucmotConfig->EIP_Adpt_TotalNumDIO;
        }
    }
    return 0;
}

void zucmotGet_trackO2world(PmHomogeneous* Pm) { *Pm = zucmotStatus->o_to_world; }

// motion status
int zucmotStatGet_percentageModeLevel() { return zucmotStatus->percentage_mode_level; }

int zucmotStatGet_threePositionEnableLimit() { return zucmotStatus->three_position_enable_limit; }

int zucmotStatGet_tcpForceLimit() { return zucmotStatus->tcp_force_limit; }

double zucmotStatGet_velScale()
{
    if (zucmotStatus->compliantType == 2 && zucmotStatus->disableForceCtrl == 0)
    {
        return fmin(zucmotStatus->net_feed_scale * zucmotStatus->velcompscale, 1.0);
    }
    else
    {
        //KLUDGE: Don't allow feed override to keep blending from overruning max velocity
        return fmin(zucmotStatus->net_feed_scale * zucmotStatus->momvelscale, 1.0);
    }
}

double zucmotStatGet_momvelScale() { return fmin(zucmotStatus->momvelscale, 1.0); }

unsigned char zucmotStatGet_enables_new() { return zucmotStatus->enables_new; }

int zucmotStatGet_ReduceMode() { return zucmotStatus->reduce_mode; }
int zucmotStatGet_still_dec() { return zucmotStatus->still_dec; }
// pos
ZucPose zucmotStatGet_convyr_pos_fb() { return zucmotStatus->disable_convyr_real_pos; }

void zucmotGetStat_jointTrqFb(double* torque)
{
    for (int joint_num = 0; joint_num < zucmotConfig->numJoints; joint_num++) { torque[joint_num] = zucmotStatus->actualTorque[joint_num]; }
}

int zucmotGet_numJoints() { return zucmotConfig->numJoints; }

int zucmotconfig_robotmodel() { return zucmotConfig->robotModel; }

int zucmotStatSet_ftConfig(int index, FtConfig ftConfig)
{
    if (index < 0 || index >= CART_DIM)
    {
        return -1;
    }
    zucmotStatus->ftConfig[index] = ftConfig;
    return 0;
}

// kinematic flags

// motion debug

int zucmotGet_axesAccLimit(int axesIndex, double* Acclimit)
{
    if (axesIndex < 0 || axesIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *Acclimit = zucmotDebug->axes[axesIndex].acc_limit;
    return 0;
}
int zucmotGet_axesVelLimit(int axesIndex, double* Vellimit)
{
    if (axesIndex < 0 || axesIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *Vellimit = zucmotDebug->axes[axesIndex].vel_limit;
    return 0;
}

int zucmotGet_IsStepping() { return zucmotDebug->stepping; }

int zucmotGet_JointVelLimit(int jointIndex, double* vellimit)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    // rtapi_print("vel_limit = %f %f %f %f %f %f %f\n",
    //             zucmotDebug->joints[0].vel_limit,
    //             zucmotDebug->joints[1].vel_limit,
    //             zucmotDebug->joints[2].vel_limit,
    //             zucmotDebug->joints[3].vel_limit,
    //             zucmotDebug->joints[4].vel_limit,
    //             zucmotDebug->joints[5].vel_limit,
    //             zucmotDebug->joints[6].vel_limit);
    *vellimit = zucmotDebug->joints[jointIndex].vel_limit;
    return 0;
}

int zucmotGet_JointAccLimit(int jointIndex, double* Acclimit)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *Acclimit = zucmotDebug->joints[jointIndex].acc_limit;
    return 0;
}

int zucmotGet_JointPosLimit(int jointIndex, double* lowBound, double* upBound)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *upBound = zucmotDebug->joints[jointIndex].max_pos_limit;
    *lowBound = zucmotDebug->joints[jointIndex].min_pos_limit;
    return 0;
}

int zucmotGet_JointActPos(int jointIndex, double* pos)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *pos = zucmotDebug->joints[jointIndex].pos_fb;
    return 0;
}

int zucmotGet_JointTrqLimit(int jointIndex, double* trq)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }

    *trq = zucmotDebug->motors[jointIndex].rated_torq * zucmotDebug->motors[jointIndex].rated_trans_ratio;
    return 0;
}

int zucmotGet_JointCmdPos(int jointIndex, double* pos)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *pos = zucmotDebug->joints[jointIndex].pos_cmd;
    return 0;
}

int zucmotGet_JointVelDir(int jointIndex, int* veldir)
{
    if (jointIndex < 0 || jointIndex >= ZUCMOT_MAX_AXIS)
    {
        return -1;
    }
    *veldir = zucmotStatus->vel_direction[jointIndex];
    return 0;
}

void zucmotSetRotaryUnlock(int jnum, int unlock)
{
    {
        // reportError(MISCELLANEOUS_ERROR,
        //             ONE_BASED(jnum),
        //             1 << jnum);
        return;
    }
    zucmot_hal_data->joint[jnum].unlock = unlock;
}

int zucmotGetRotaryIsUnlocked(int jnum) { return zucmot_hal_data->joint[jnum].is_unlocked; }

int zucmot_GetIOStatus(IODevType devtype, int di_Index, int* value)
{
    if (devtype == 0)
    {  // basic IO condition
        if (di_Index >= 0 && di_Index < ZUCMOT_MAX_DIO)
        {
            *value = zucmotStatus->synch_di[di_Index];
            return 0;
        }
    }
    else if (devtype == 1)
    {  // tool IO condition //
        if (di_Index >= 0 && di_Index < 2)
        {
            int atlIOStat = (zucmotStatus->tio_status.DI_buffer >> di_Index) & 0x01;
            *value = atlIOStat;
            return 0;
        }
    }
    else if (devtype == 2)
    {  // ext IO condition
        if (di_Index >= 0 && di_Index < MAX_EXTIO_DI_NUM)
        {
            *value = zucmotStatus->extio_stat.extDI[di_Index];
            return 0;
        }
    }
    else if (devtype == 4)
    {  // modbus condition
        if (di_Index >= 0 && di_Index < ZUCMOT_MAX_MODBUS_DIO)
        {
            *value = zucmotStatus->synch_mb_slave_di[di_Index];
            return 0;
        }
    }
    else if (devtype == 5)
    {  // profinet condition
        if (di_Index >= 0 && di_Index < ZUCMOT_MAX_PNDev_DIO)
        {
            *value = zucmotStatus->synch_pn_dev_di[di_Index];
            return 0;
        }
    }
    else if (devtype == 6)
    {  // ethernet/ip condition
        if (zucmotStatus->eip_mode == 1 && di_Index >= 0 && di_Index < zucmotStatus->eip_di_num)
        {
            int byteNum = di_Index / 8;
            di_Index -= byteNum * 8;
            *value = (0x01 & (0xff & (zucmotStatus->synch_eip_in[byteNum] >> di_Index)));
            return 0;
        }
        if (zucmotStatus->eip_mode == 0 && di_Index >= 0 && di_Index < ZUCMOT_MAX_EIP_DIO)
        {
            *value = zucmotStatus->synch_eip_adpt_di[di_Index];
            return 0;
        }
    }
    return -1;
}

int zucmot_SetDO(IODevType devtype, int di_Index, int value)
{
    // if (devtype == IODevTYPE_TIO)
    // {
    //     if (di_Index >= 0 && di_Index < 2)
    //     {
    //         return tio_set_dout(di_Index, value);
    //     }
    // }
    // else
    // {
    //     return write_DIO_i(devtype, di_Index, value);
    // }
    // return -1;
    return 0;
}

int zucmot_SetAO(IODevType devtype, int di_Index, float value)
{
    return 0;
    // return write_AIO_f(devtype, di_Index, value);
}

double zucmot_GetCfgTcpVelLimit() { return zucmotConfig->safetyCfg.tcpVelLimit; }

int zucmot_GetSafeZoneImformation(int* autoEnable, int* openEnable, int* safezone_Enable, int* safezone_Dengrous, int* elbow_sphere_radius, SafeZone* safezone)
{
    *autoEnable = zucmotConfig->safetyCfg.zone.autoEnable;
    *openEnable = zucmotConfig->safetyCfg.zone.openEnable;
    *safezone_Enable = zucmotConfig->safeZoneEnable;
    *safezone_Dengrous = zucmotStatus->safezone_dengrous;
    *elbow_sphere_radius = zucmotConfig->safetyCfg.elbow_sphere_radius;
    *safezone = zucmotConfig->safetyCfg.zone;
    return (*autoEnable || *openEnable);
}

int zucmot_checkSafezone(SafeZone zone,
                         ZucPose curPose,
                         ZucPose cur_elbow_Pose,
                         int elbow_wrist_flag,
                         double R_elbow,
                         ZucPose current_user_offset,
                         int pre_flag,
                         checksafefalg_out* checksafefalg)
{
    return checkSafezone(zone, curPose, cur_elbow_Pose, elbow_wrist_flag, R_elbow, current_user_offset, pre_flag, checksafefalg);
}

int zucmot_SetCheck_safezone_prediction()
{
    zucmotStatus->check_safezone_prediction = 1;
    return 0;
}

int zucmot_GetTrajCycleTime(double* trajCycleTime)
{
    *trajCycleTime = zucmotConfig->trajCycleTime;
    return 0;
}

int zucmot_set_status_jvel_limit(int is_limit, int index)
{
    zucmotStatus->is_jointvel_limited[index] = is_limit;
    // rtapi_print("zucmotStatus->is_jointvel_limited[%d] = %d\n", index, is_limit);
    return 0;
}

int zucmot_set_status_cpos_cmd(ZucPose cartPose)
{
    zucmotStatus->carte_pos_cmd.tran = cartPose.tran;
    zucmotStatus->carte_pos_cmd.a = cartPose.a;
    zucmotStatus->carte_pos_cmd.b = cartPose.b;
    zucmotStatus->carte_pos_cmd.c = cartPose.c;
    return 0;
}

int zucmot_set_status_cpos_desired(ZucPose cartPose)
{
    zucmotStatus->carte_pos_desired.tran = cartPose.tran;
    zucmotStatus->carte_pos_desired.a = cartPose.a;
    zucmotStatus->carte_pos_desired.b = cartPose.b;
    zucmotStatus->carte_pos_desired.c = cartPose.c;
    return 0;
}

int zucmot_get_status_cpos_cmd(ZucPose* cartPose)
{
    cartPose->tran = zucmotStatus->carte_pos_cmd.tran;
    cartPose->a = zucmotStatus->carte_pos_cmd.a;
    cartPose->b = zucmotStatus->carte_pos_cmd.b;
    cartPose->c = zucmotStatus->carte_pos_cmd.c;
    return 0;
}

int zucmot_get_status_cpos_fb(ZucPose* cartPose)
{
    cartPose->tran = zucmotStatus->carte_pos_fb.tran;
    cartPose->a = zucmotStatus->carte_pos_fb.a;
    cartPose->b = zucmotStatus->carte_pos_fb.b;
    cartPose->c = zucmotStatus->carte_pos_fb.c;
    return 0;
}

int zucmot_set_jointpos_cmd(double* jpos_cmd)
{
    for (int jointIndex = 0; jointIndex < ZUCMOT_MAX_JOINTS; jointIndex++) { zucmotDebug->joints[jointIndex].pos_cmd = jpos_cmd[jointIndex]; }

    return 0;
}